#pragma once


#include <vector>
#include <fstream>
#include <iostream>


#include <opencv2/core/core.hpp>

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/Dense>

#include <sophus/se3.hpp>
#include <sophus/so3.hpp>

#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/base_binary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include "g2o/core/sparse_block_matrix.h"
#include "g2o/core/optimization_algorithm_gauss_newton.h"
#include "g2o/solvers/linear_solver_eigen.h"
#include "g2o/core/robust_kernel_impl.h"
#include "g2o/solvers/linear_solver_dense.h"


// from slam 14 book
using namespace std;



class VertexPose : public g2o::BaseVertex<6, Sophus::SE3d> {
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

  virtual void setToOriginImpl() override {
    _estimate = Sophus::SE3d();
  }

  // 使用左绕动
  virtual void oplusImpl(const double *update) override {
    Eigen::Matrix<double, 6, 1> update_eigen;
    update_eigen << update[0], update[1], update[2], update[3], update[4], update[5];
    _estimate = Sophus::SE3d::exp(update_eigen) * _estimate;// 李代数转李群，左乘更新的
  }

  virtual bool read(istream &in) override {}

  virtual bool write(ostream &out) const override {}
};

// 定义地图点的边
class VertexPoint3 : public g2o::BaseVertex<3, Eigen::Vector3d> {
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  // 重置
  virtual void setToOriginImpl() override {
    _estimate << 0, 0, 0;
  }

  // 更新(相加)
  virtual void oplusImpl(const double *update) override {
    _estimate += Eigen::Vector3d(update);
  }

  // 存盘和读盘：留空
  virtual bool read(istream &in) {}
  virtual bool write(ostream &out) const {}
};

// class EdgeMono : public g2o::BaseBinaryEdge<2, Eigen::Vector2d, g2o::VertexSBAPointXYZ, VertexPose>
// 计算边的部分  误差维度 误差类型 顶点1  顶点2
class EdgeProjection : public g2o::BaseBinaryEdge<2, Eigen::Vector2d, VertexPoint3, VertexPose> {
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
  // 只需要传进来内参矩阵
  EdgeProjection(const Eigen::Matrix3d &K) : _K(K) {}
  Eigen::Vector2d outError(){
    return _error;
  }
  virtual void computeError() override {
    const VertexPose *vPos = static_cast<VertexPose *> (_vertices[1]);
    const VertexPoint3 *vPoint = static_cast<VertexPoint3 *> (_vertices[0]);
    Sophus::SE3d T = vPos->estimate();// 估计值
    Eigen::Vector4d point4;
    point4.block<3, 1>(0, 0) = vPoint->estimate();
    point4[3] = 1;
    Eigen::Vector4d pinCam = T * point4;
    Eigen::Vector3d pos_pixel = _K * (pinCam.block<3,1>(0,0) / pinCam(2, 1));// 转换到图像坐标系
    _error = _measurement - pos_pixel.head<2>();// 误差
  }

  virtual void linearizeOplus() override {// 计算雅克比
    const VertexPose *vPos = static_cast<VertexPose *> (_vertices[1]);
    const VertexPoint3 *vPoint = static_cast<VertexPoint3 *> (_vertices[0]);
    Sophus::SE3d T = vPos->estimate();// 估计值
    auto TMatrix = T.matrix();
    Eigen::Vector3d point3 = vPoint->estimate();

    
    double fx = _K(0, 0);
    double fy = _K(1, 1);
    double cx = _K(0, 2);
    double cy = _K(1, 2);

    // 在camera坐标系下的值
    Eigen::Vector3d point3inCamera = TMatrix.block<3, 3>(0, 0) * point3 + TMatrix.block<3, 1>(0, 3);
    double X = point3inCamera[0];
    double Y = point3inCamera[1];
    double Z = point3inCamera[2];
    double Z2 = Z * Z;
    // 对三维点导数
    
    // 对位姿点的导数
    Eigen::Matrix<double, 2, 2> dednp;// 误差对单位平面点求导
    dednp << -fx, 0, 0, -fy, 0;
    Eigen::Matrix<double, 2, 3> dnpdpc; // 归一化平面点对在camera下的三维点导数
    dnpdpc << 1./ Z, 0, - X/Z2, 0., 1./Z, -Y/Z2;
    Eigen::Matrix<double, 3, 6> dpcdTc; // 相机坐标系下的三维点对位姿的导数 
    dpcdTc.block<3,3>(0, 0) = Eigen::Matrix3d::Identity();
    dpcdTc.block<3,3>(0, 3) = -Sophus::SO3d::hat(point3inCamera);

    _jacobianOplusXi = dednp * dnpdpc * TMatrix.block<3, 3>(0, 0);
    _jacobianOplusXj = dednp * dnpdpc * dpcdTc;
  }

  virtual bool read(istream &in) override {}

  virtual bool write(ostream &out) const override {}

private:

  Eigen::Matrix3d _K;
};



